/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package kuti.grp5;

import java.awt.geom.Point2D;
import java.util.ArrayList;
import java.util.Iterator;

/**
 *
 * @author Pierre & Jonathan
 */
public class KutiMap {

    private Point2D crtPos; // Kuti's relative current position
    private double orientation; // Kuti's relative orientation
    private ArrayList<Point2D> mapPowerUnit = new ArrayList<Point2D>(); // Kuti's relative power unit's map 
    private ArrayList<Point2D> mapObstacle = new ArrayList<Point2D>();  // Kuti's relative obstacle's map
    private ArrayList<Point2D> path = new ArrayList<Point2D>(); // Path to power unit
    private boolean isPowerUnitFound = false; // rapidly find out if a power unit has been found
    private Point2D bestPoint = null;   // Best power unit point 

    public KutiMap() {
        crtPos = new Point2D.Double(0.0, 0.0);
        orientation = 0.0;
    }

    /**
     * Use to find the path to the closest power station
     *
     * @param newSearch: say if it's a new request or want the next step of the
     * path
     * @return minvalue if there is no power station discovered yet or the
     * direction to choose to go to a power station
     */
    public double chooseDirection(boolean newSearch) {
        if (newSearch && isPowerUnitFound) {
            chooseBestPowerUnit();
        }
        return orientationToPoint(isPowerUnitFound);
    }

    /**
     *
     * @return The closest point of a power unit
     */
    private boolean chooseBestPowerUnit() {
        Iterator<Point2D> it = mapPowerUnit.iterator();
        Point2D point;
        boolean retour = false;
        double minDist = Double.MAX_VALUE;
        double dist = 0.0;
        while (it.hasNext()) {
            point = it.next();
            dist = point.distance(crtPos);
            if (dist < minDist) {
                bestPoint = point;
                minDist = dist;
                retour = true;
            }
        }
        return retour;
    }

    /**
     * set the current orientation
     *
     * @param orientation current new orientation
     */
    public void setOrientation(double orientation) {
        this.orientation += orientation;
        this.orientation = Driver.PiMinusToPi(this.orientation);

    }

    /**
     * Add a power unit in the "intern map"
     *
     * @param newPoint distance from current poin to distant power unti's point
     */
    public void addPowerUnitPoint(double distToPoint) {
        double y = crtPos.getY() + Math.sin(orientation) * distToPoint;
        double x = crtPos.getX() + Math.cos(orientation) * distToPoint;
        mapPowerUnit.add(new Point2D.Double(x, y));
        isPowerUnitFound = true;
    }

    /**
     * Add a obstacle point in the "intern map"
     *
     * @param distToPoint distance from current point to distant point
     */
    public void addObstaclePoint(double distToPoint) {
        double y = crtPos.getY() + Math.sin(orientation) * distToPoint;
        double x = crtPos.getX() + Math.cos(orientation) * distToPoint;
        mapObstacle.add(new Point2D.Double(x, y));
    }

    /**
     * Give the orientation to go to the best Power Unit point
     *
     * @param powerUnitFound true if powerUnit is found
     * @return new orientation to adopt or min_value if no power unit was found
     */
    private double orientationToPoint(boolean powerUnitFound) {
        if (powerUnitFound) {
            double aAngle = absAngle();
            if (aAngle > orientation) {
                return Math.abs(orientation - aAngle);
            } else if (aAngle < orientation) {
                return -(orientation - aAngle);
            } else {
                return 0.0;
            }
        }
        return Double.MIN_VALUE;
    }

    /**
     *
     * @return the absolute angle between the current position and the best
     * power unit point
     */
    private double absAngle() {
        if ((bestPoint.getX() - crtPos.getX()) < 0) {
            if (bestPoint.getY() - crtPos.getY() >= 0) {
                return Math.PI - Math.asin((bestPoint.getY() - crtPos.getY()) / (bestPoint.distance(crtPos)));
            } else {
                return Math.PI + Math.abs(Math.asin((bestPoint.getY() - crtPos.getY()) / (bestPoint.distance(crtPos))));
            }
        }

        return Math.asin((bestPoint.getY() - crtPos.getY()) / (bestPoint.distance(crtPos)));
    }

    /**
     * This method updates the CrtPos that will be used to know the current
     * relativ position of the Kuti.
     *
     * @param speed
     */
    public void updateCrtPos(double speed) {
        double y = Math.sin(orientation) * speed;
        double x = Math.cos(orientation) * speed;
        crtPos.setLocation(crtPos.getX() + x, crtPos.getY() + y);
    }

    /**
     * Set the current position
     *
     * @param crtPos
     */
    public void setCrtPos(Point2D crtPos) {
        this.crtPos = crtPos;
    }

    /**
     *
     * @return the current position
     */
    public Point2D getCrtPos() {
        return crtPos;
    }

    /**
     * give true if a power unit is found
     *
     * @return
     */
    public boolean getIsPowerUnitFound() {
        return isPowerUnitFound;
    }

    /**
     * Update the power unit position and set the current position
     */
    public void updatePowerUnitPos() {
        if (bestPoint == null) {
            bestPoint = new Point2D.Double(crtPos.getX(), crtPos.getY());

        } else {
            bestPoint.setLocation(crtPos);
        }
    }

    public Point2D getPowerUnitPos(){
        return bestPoint;
    }
}
